#include "Poly.h"
#include "SDL_opengl.h"
#include <algorithm>
#include <iostream>
#include <limits>
#include <cmath>

using namespace std;

CollisionAxis testCollision( Poly& p1, Poly& p2)
{
	CollisionAxis collisionAxis1 = p1.findSeparatingAxis( p2);
	if ( collisionAxis1.overlap == 0.0f )
		return collisionAxis1;
	
	CollisionAxis collisionAxis2 = p2.findSeparatingAxis(p1);
	if ( collisionAxis2.overlap == 0.0f )
		return collisionAxis2;
	
	// Draw collision points in object 1
	for ( vector<Vector2*>::iterator it = collisionAxis1.contactPoints.begin(); it != collisionAxis1.contactPoints.end(); it++)
	{
		    glMatrixMode( GL_MODELVIEW );
		glColor3f(1,0,0);
		glBegin(GL_POLYGON);
		cout << (*it)->x << ", " << (*it)->y << endl;
		glVertex2f( (*it)->x, (*it)->y );
		glVertex2f( (*it)->x, (*it)->y + 0.5f );
		glVertex2f( (*it)->x + 0.5f, (*it)->y + 0.5f);
		glVertex2f( (*it)->x + 0.5f, (*it)->y );
		glEnd();
	}
	
	if ( collisionAxis1.overlap < collisionAxis2.overlap )
	{
		// If adjusting based on a perpendicular of the first object,
		// then the translation needs to be in the opposite direction
		collisionAxis1.overlap = -collisionAxis1.overlap;
		return collisionAxis1;
	}
	
	return collisionAxis2;
}

Poly::Poly( vector<Vector2> verts ): speed(0.1f), xVel(0.0f), yVel(0.0f), vertices(verts)
{
	calculateCentroid();
}


//Poly::Poly( PolyType type )
//{
//	switch (type)
//	{
//		case SQUARE:
//			break;
//		case TRIANGLE:
//			break;
//	}
//}

void Poly::addVertex( Vector2 vert )
{
	vertices.push_back( vert );
}

void Poly::draw()
{
	if (vertices.size() < 3)
		throw "NOT A POLYGON!";
	
	glColor3f( 0, 1, 0);
	glBegin(GL_POLYGON);
	for ( vector<Vector2>::const_iterator it = vertices.begin(); it != vertices.end(); it++)
	{
		glVertex2f( it->x, it->y );
	}
	glEnd();
}

bool absFloatCompare(float i, float j)
{
	return abs(i) < abs(j);
}

CollisionAxis Poly::findSeparatingAxis ( Poly& p )
{
	/*
	 for each poly:
		for each side:
			find perp of side
			project both polys to perp
			if overlap:
				continue
			else:
				separating axis found, no collision
	 */
	
	// This is the candidate axis for collision response
	CollisionAxis collisionAxis( Vector2::ZERO, numeric_limits<float>::max() );
	
	// This vector is the vector separating the two polygons.
	Vector2 distance = p.getCentroid() - centroid;
	
	// Loop through this poly's verts to test each side
	for ( vector<Vector2>::const_iterator it = vertices.begin(); it != vertices.end(); it++ )
	{
		vector<Vector2>::const_iterator nextVert = it + 1;
		if ( nextVert == vertices.end() )
			nextVert = vertices.begin();
		
		// now find the side/line which forms the potential separating axis
		Vector2 axis = ( (*nextVert) - (*it) );
		
		// Find perpendicular to axis
		Vector2 perp = Vector2( -axis.y, axis.x);
		
		// If dot product of separation and perpendicular is negative, then the perpindicular
		// is facing away from the other object, and hence can't be a perpendicular of a
		// separating axis.
		if ( (perp * distance) < 0.0f )
		{
			continue;
		}
		
		// Project both polys to the perpendicular
		perp.normalize();
		
		vector<float> projected1;
		
		// Project each vert of poly1 to the line
		for ( vector<Vector2>::const_iterator it = vertices.begin(); it != vertices.end(); it++ )
		{
			// add the dot product
			projected1.push_back( perp * (*it) );
		}
		
		float max1 = *max_element(projected1.begin(), projected1.end() );
		float min1 = *min_element( projected1.begin(), projected1.end() );
		
		// Project each vert of poly2
		vector<Vector2>* verts2 = p.getVertices();
		vector<float> projected2;
		
		for ( vector<Vector2>::const_iterator it = verts2->begin(); it != verts2->end(); it++ )
		{
			// add the dot product
			projected2.push_back( perp * (*it) );
		}
		
		float max2 = *max_element(projected2.begin(), projected2.end() );
		float min2 = *min_element( projected2.begin(), projected2.end() );
		
		if (max1 < min2 || min1 > max2)
		{
			// NO COLLISION, AXIS FOUND
			return CollisionAxis( Vector2(0.0f,0.0f), 0.0f );
		}
		
		float overlap;
		if (max1 > max2)
		{
			overlap = max2 - min1;
		}
		else
		{
			overlap = max1 - min2;
		}
		
		// Compare overlap to minimum overlap
		if (overlap < collisionAxis.overlap)
		{
			collisionAxis.axis = perp;
			collisionAxis.overlap = overlap;
		}
	}
	
	//Getting to this point means that no separating axis was found
	// thus return the axis with the smallest projection overlap
	
	vector<float> projected;
	
	// Project each vert of this poly to the collision line
	for ( vector<Vector2>::const_iterator it = vertices.begin(); it != vertices.end(); it++ )
	{
		// add the dot product
		projected.push_back( collisionAxis.axis * (*it) );
	}
	
	// Now find the smallest absolute value
	float min = *min_element( projected.begin(), projected.end(), absFloatCompare);
	
	const float epsilon = 1.0E-8;
	//vector<Vector2*> contactPoints;
	
	for ( int i = 0; i < projected.size(); i++ )
	{
		// If the absolute value of the dot product is less than the minimum plus a 
		// small threshold, add it to the contact points vector
		if ( projected[i] < (min + epsilon) )
		{
			collisionAxis.contactPoints.push_back( &vertices[i] );
			if ( i >= 2)
				break;
		}
	}
	return collisionAxis;
	
}

vector<Vector2>* Poly::getVertices()
{
	return &vertices;
}

void Poly::translate(Vector2 tv)
{
	for ( vector<Vector2>::iterator it = vertices.begin(); it != vertices.end(); it++)
	{
		it->x += tv.x;
		it->y += tv.y;
	}
	calculateCentroid();
}

void Poly::handleEvent(SDL_Event* event)
{
	if (event->type == SDL_KEYDOWN)
	{
		switch( event->key.keysym.sym )
		{
				
			case SDLK_UP:

				yVel += speed;
				break;
			case SDLK_DOWN:
				yVel -= speed;
				break;
			case SDLK_LEFT:
				xVel -= speed;
				break;
			case SDLK_RIGHT:
				xVel += speed;
				break;
		}
	}
	else if (event->type == SDL_KEYUP)
	{
		switch( event->key.keysym.sym )
		{
			case SDLK_UP:
				yVel -= speed;
				break;
			case SDLK_DOWN:
				yVel += speed;
				break;
			case SDLK_LEFT:
				xVel += speed;
				break;
			case SDLK_RIGHT:
				xVel -= speed;
				break;
		}
	}
	
}

void Poly::update( Poly& p )
{
	translate( Vector2(xVel, yVel) );
	
	CollisionAxis cAxis = testCollision( (*this), p );
	if ( cAxis.overlap != 0.0f )
	{
		translate( cAxis.axis * cAxis.overlap );
		
		// PHYSICS RESPONSE
		Vector2 xAxis = p.getCentroid() - centroid;
		
	}
}

void Poly::calculateCentroid()
{
	Vector2 sum(0.0f, 0.0f);
	for ( vector<Vector2>::const_iterator it = vertices.begin(); it != vertices.end(); it++)
	{
		sum = sum + (*it);
	}
	
	centroid = sum / vertices.size();
}

Vector2 Poly::getCentroid()
{
	return centroid;
}
